#include <stdio.h>
#include <unistd.h>
#include "ohos_init.h"
#include "cmsis_os2.h"
#include "lwip/sockets.h"
#include "wifi_sta_connect.h"
#include "spark_arm.h"
#include "spark_base.h"
#include "spark_cmd_transfer.h"
#include "ws2812_module.h"


#include "parse_json.h"
#define SELECT_WIFI_SSID "NXROBO"
#define SELECT_WIFI_PASSWORD "rightright"
#define SELECT_WIFI_IPADDRESS "192.168.100.248"
#define SELECT_WIFI_GATEWAY "192.168.100.1"
#define _PROT_ 10006
#define TCP_BACKLOG 10
char *default_wifi = "set_wifi:{\"wifi_master\":{\"ssid\":\"NXROBO\",\"password\":\"rightright\",\"ipaddress\":\"192.168.100.247\",\"gateway\":\"192.168.100.1\"}}";

//在sock_fd 进行监听，在 new_fd 接收新的链接
int sock_fd, new_fd;
char recvbuf[128];
char sendbuf[128];
//1
unsigned int count_rev = 0;
unsigned int count = 0;
unsigned int count_rev_old = 0;

char *buf = "Hello! I'm SPARK-HM-PI TCP Server!";

extern BaseStatus g_base_status;
extern int keep_going;
extern int new_status;



void Recv_Msg_Handle(char *recvbuf)
{
	//2
	count_rev++;
	if(new_status!=2)
	{
		new_status = 2;
		keep_going = 0;
	}
    //运动指令
	if(strlen(recvbuf) == 1 && strcmp(recvbuf,"0")>=0 && strcmp(recvbuf,"5")<0)
	{
		//底盘状态切换
		Base_Status_Change(recvbuf);
	}
    //机械臂指令
	else if(!strcmp("5",recvbuf))
	{
		// Base_Status_Change("0");
		Arm_Grasp();
	}
	else if(!strcmp("6",recvbuf))
	{
		// Base_Status_Change("0");
		Arm_Release();
	}
	else if(!strcmp("7",recvbuf))
	{
		// Base_Status_Change("0");
		Arm_Release_two();
	}
	else if(!strcmp("open",recvbuf))
	{
		new_status = 3; //新状态为打开虚拟门。
		keep_going = 0;
		printf("receive open!\n");
	}
	else if(!strcmp("close",recvbuf))
	{
		new_status = 4; //新状态为已连接网络。
		keep_going = 0;
		printf("receive close!\n");
	}
	else
	{
		Cmd_Transfer(recvbuf, strlen(recvbuf)+1);
	}
}

 void TCPServerTask(void)
{
	//服务端地址信息
	struct sockaddr_in server_sock;

	//客户端地址信息
	struct sockaddr_in client_sock;
	int sin_size;

	struct sockaddr_in cli_addr;

	//创建socket
	if ((sock_fd = socket(AF_INET, SOCK_STREAM, 0)) == -1)
	{
		perror("socket is error\r\n");
		exit(1);
	}

	bzero(&server_sock, sizeof(server_sock));
	server_sock.sin_family = AF_INET;
	server_sock.sin_addr.s_addr = htonl(INADDR_ANY);
	server_sock.sin_port = htons(_PROT_);

	//调用bind函数绑定socket和地址
	if (bind(sock_fd, (struct sockaddr *)&server_sock, sizeof(struct sockaddr)) == -1)
	{
		perror("bind is error\r\n");
		exit(1);
	}

	//调用listen函数监听(指定port监听)
	if (listen(sock_fd, TCP_BACKLOG) == -1)
	{
		perror("listen is error\r\n");
		exit(1);
	}

	printf("start accept\n");
	//调用accept函数从队列中
	while (1)
	{
		sin_size = sizeof(struct sockaddr_in);

		if ((new_fd = accept(sock_fd, (struct sockaddr *)&client_sock, (socklen_t *)&sin_size)) == -1)
		{
			perror("accept");
			continue;
		}

		// cli_addr = malloc(sizeof(struct sockaddr));

		printf("accept addr\r\n");

		memcpy(&cli_addr, &client_sock, sizeof(struct sockaddr));


		//处理目标
		ssize_t ret;

		while (1)
		{
			//连接断开防止跳不出循环
			if ((ret = recv(new_fd, recvbuf, sizeof(recvbuf), 0)) <= 0)
			{
				printf("recv error \r\n");
				//防止连接断开是机器人还在动
				Base_Status_Change("0");
				break;
			}
			printf("recv :%s\r\n", recvbuf);
			// printf("%d %d %d\r\n",strlen(recvbuf),strcmp(recvbuf,"-1"),strcmp("5",recvbuf));
			//处理收到的信息
			Recv_Msg_Handle(recvbuf);
			bzero(recvbuf,sizeof(recvbuf));

		}
		closesocket(new_fd);
	}
}

//SparkMoveTask任务，用于手机
 void SparkMoveTask(void)
{
	BaseStatus base_current_status = BASE_STATUS_STOP;
	while(1)
	{
	
		//3
		if(count > 20)
		{
			count = 0;
			if (count_rev == count_rev_old)
			{
				g_base_status = BASE_STATUS_STOP;
			}
			else
			{
				count_rev_old = count_rev;
			}
		}
		switch (g_base_status)
		{
		case BASE_STATUS_STOP:
			if(g_base_status != base_current_status)
			{
				Base_Stop();
			}
			break;
		case BASE_STATUS_FORWARD:
			Base_Forword();
			break;
		case BASE_STATUS_BACKWARD:
			Base_Backward();
			break;
		case BASE_STATUS_LEFT:
			Base_Left();
			break;
		case BASE_STATUS_RIGHT:
			Base_Right();
			break;
		default:
			break;
		}
		base_current_status = g_base_status;
		usleep(1000*100);
		count++;
	}
}

char recv_msg[200];
char first_time = 1;
void RequestWifiCmdTask(void)
{
	char *request_cmd = "wifi_msg";
	new_status = 0; //新状态为未连接网络。
	keep_going = 0;
	while(first_time)
	{
		Cmd_Transfer(request_cmd, strlen(request_cmd)+1);
		printf("send:%s\n",request_cmd);
		sleep(5);
	}
}

void get_Uart_Msg(void)
{

	static osThreadId_t thread_id = NULL;
	static char network_str[200];
	char tmp_msg[10];
	char output_ssid[50];
	char output_password[50];
	char output_ipaddress[20];
	char output_gateway[20];

	osThreadAttr_t attr;
	attr.name = "TCPServerTask";
	attr.attr_bits = 0U;
	attr.cb_mem = NULL;
	attr.cb_size = 0U;
	attr.stack_mem = NULL;
	attr.stack_size = 10240;
	attr.priority = osPriorityNormal;

	Uart_ReceiveData(recv_msg, sizeof(recv_msg));
	printf("recv_msg:%s\r\n", recv_msg);

	memset(tmp_msg, 0, sizeof(tmp_msg));
	memcpy(tmp_msg, recv_msg, sizeof(tmp_msg) - 1);
	if (!strcmp(tmp_msg, "0"))
	{
		g_arm_status = ARM_STATUS_RELEASE;
	}
	else if (!strcmp(tmp_msg, "1"))
	{
		g_arm_status = ARM_STATUS_GRASP;
	}
	else if (strcmp(tmp_msg, "set_wifi:") == 0)
	{

		printf("it is set_wifi:\n");
		if (first_time)
		{

			if (strcmp(network_str, recv_msg) != 0)
			{
				strcpy(network_str, recv_msg);
				printf("network_str:%s, %d\r\n", network_str, strlen("set_wifi:"));
				cjson_parse_embed_key_str(network_str + strlen("set_wifi:"), "wifi_master", "ssid", output_ssid);
				cjson_parse_embed_key_str(network_str + strlen("set_wifi:"), "wifi_master", "password", output_password);
				cjson_parse_embed_key_str(network_str + strlen("set_wifi:"), "wifi_master", "ipaddress", output_ipaddress);
				cjson_parse_embed_key_str(network_str + strlen("set_wifi:"), "wifi_master", "gateway", output_gateway);
				if(WifiConnect(output_ssid, output_password, output_ipaddress, output_gateway)==-1)
					return;
				first_time = 0;
				new_status = 1; //新状态为已连接网络。
				keep_going = 0;
				thread_id = osThreadNew((osThreadFunc_t)TCPServerTask, NULL, &attr);
				if (thread_id == NULL)
				{
					printf("[TCPServerDemo] Falied to create TCPServerTask!\n");
				}
			}
		}
	}
	else
	{
		g_arm_status = ARM_STATUS_ERROR;
		printf("get other msg:%s\n", recv_msg);
	}
	bzero(recv_msg, sizeof(recv_msg));
}

void GetArmStatusTask(void)
{
	ssize_t ret;
    while(1)
    {
        //Update_Arm_Status();
		get_Uart_Msg();
		Get_Arm_Status(sendbuf);
		// sleep(2);
		if(new_fd<=0)
			continue;
		if ((ret = send(new_fd, sendbuf, strlen(sendbuf) + 1, 0)) == -1)
		{
			perror("send : "); 
		}
		bzero(sendbuf,sizeof(sendbuf));
    }
}


void SparkCmdMasterEntry(void)
{
	//初始化
	Spark_Init();
	osThreadAttr_t attr;

	attr.name = "TCPServerTask";
	attr.attr_bits = 0U;
	attr.cb_mem = NULL;
	attr.cb_size = 0U;
	attr.stack_mem = NULL;
	attr.stack_size = 10240;
	attr.priority = osPriorityNormal;

	// if (osThreadNew((osThreadFunc_t)TCPServerTask, NULL, &attr) == NULL)
	// {
	// 	printf("[TCPServerDemo] Falied to create TCPServerTask!\n");
	// }

	attr.name = "SparkMoveTask";
	attr.stack_size = 1024*2;
	if (osThreadNew((osThreadFunc_t)SparkMoveTask, NULL, &attr) == NULL)
	{
		printf("[SparkMoveTask] Falied to create SparkMoveTask!\n");
	}

	attr.name = "GetArmStatusTask";
	attr.stack_size = 1024*4;
	if (osThreadNew((osThreadFunc_t)GetArmStatusTask, NULL, &attr) == NULL)
	{
		printf("[GetArmStatusTask] Falied to create GetArmStatusTask!\n");
	}
	attr.name = "RequestWifiCmdTask";
	attr.stack_size = 1024*4;
	if (osThreadNew((osThreadFunc_t)RequestWifiCmdTask, NULL, &attr) == NULL)
	{
		printf("[RequestWifiCmdTask] Falied to create RequestWifiCmdTask!\n");
	}
	attr.name = "WS2812Task";
	attr.stack_size = 1024*4;
	if (osThreadNew((osThreadFunc_t)WS2812Task, NULL, &attr) == NULL)
	{
		printf("[WS2812Task] Falied to create WS2812Task!\n");
	}


}

APP_FEATURE_INIT(SparkCmdMasterEntry);
